/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file axisobject.h
 *  功能:
 * 轴对象的管理和对外接口。
 *
 *                                                                         *
 ***************************************************************************/
#ifndef AxisOBJECT_H
#define AxisOBJECT_H
#include "AxisDefines.h"
#include "qqueue.h"
#include <velocityprofile_trap.hpp>
#include <velocityprofile_traphalf.hpp>
#include <velocityprofile_spline.hpp>
#include <velocityprofile_rect.hpp>
#include "MacroDefine/GeneralDefine.h"
#include "motionmessage.h"
#include "ecattype.h"

struct simple_tp_t;
enum E_MIX_STAGE
{
    E_MIX_STAGE_NONE,
    E_MIX_STAGE_READY,
    E_MIX_STAGE_MIXING,
};



struct AxisMixControlInfo
{
    E_MIX_STAGE stage;
    double segmentDurationLeft;
    double beginDuration;
};

struct AxisTrajeAttribute
{
    KDL::VelocityProfile* trajComp;
    double timeDuration;
    int cnt;
    double endCutoffTime;
    double accumulatedTime;


};


using namespace KDL;

/**************************************
//单位统一说明：
//关节采用　s , 　mm ,　degree。
代码的计算与单位无关，只要单位统一，结果就是正确的。
**************************************/

class FilteringAlgorithm;
/**
 * @brief The AxisObject class 单个轴的控制
 */
class AxisObject
{
public:
    AxisObject(int robotIdIn, JointAttribute axisAttributeIn,MotionMessage* motionMessageIn);

public:
    //tp
    int axisSimpleLoopOnce();
    int getAxisSimpleTpInfo(SimpleTpInfo &infoOut);
    int controlAxisSimpleTp(int isEnable);
    int setAxisSimpleTpTarget(double targetPosIn,double maxVelIn,double maxAccIn);
    E_AXIS_PLAN_MODE getAxisPlanMode();
    int setAxisPlanMode(E_AXIS_PLAN_MODE modeIn);
    int ceaseAxisObject();

    double getIdealTorque();
    int setIdealTorque(double torqueIn);
    int setT1Ratio(double ratioIn);
    int setPlanStartPosition_byPositionCommand();
    int setPlanStartPosition(double positionIn);
    double getMixDurationLeft();
    double getPositionCommand_filter();
    double getVelLimitRatio();
    double getStopTime();
    int setAxisProperty(struct AxisProperty_hmi axisPropertyIn);
    int setAxisPositionLimit(double minPositionIn, double maxPositionIn,double approachDistanceIn);
    int setMaxVel(double maxVelIn);
    int setAccelerationRatio(double ratioIn);
    int setMaxAcceleration(double maxAccIn);
    double getVelCompensation();
    int setServoOnStatus(bool isOn);
    int axisFeedbackCheck();//只检查反馈．只要使能开启，该函数就会定时运行
    int isPositionCommandApprochLimit();
    int restoreControlMode();
    int getMotorControlMode(E_MOTOR_CONTROL_MODE &modeOut);
    int setMotorControlMode(E_MOTOR_CONTROL_MODE modeIn);
    /**
     * @brief loadAxisAttribute 加载轴属性
     * @param axisAttributeIn
     * @return
     */
    int loadAxisAttribute(JointAttribute axisAttributeIn);

    /**
     * @brief returnAxisCommandData 获取当前axisCommandData
     * @param returnedPosition
     * @param returnedVel
     * @param returnedAcc
     * @param returnedTorque
     * @return
     */
    int returnAxisCommandData(double &returnedPosition, double &returnedVel, double &returnedAcc, double &returnedTorque );
    int returnAxisFeedbackData(double &returnedPosition, double &returnedVel, double &returnedTorque );
    int returnAxisFollowOffsetData(double &returnedPosition, double &returnedVel,double &returnedTorque );
    /**
     * @brief setTimeAxisScale 设置时间缩放比例　０～１
     * @param ratio
     * @return
     */
    int setTimeAxisScale(double ratio);
    /**
     * @brief setDeltaTime 设置单步时间长
     * @param time
     * @return
     */
    int setDeltaTime(double time);
    /**
     * @brief moveOneStep 计算一步
     * @return 　０没轨迹或超极限　１有轨迹
     */
    int moveOneStep();
    int moveOneStepByTrajIndex(int trajIndex=0);
    int setVelCompensation(double velIn);//速度补偿
    int setTorqueCompensation(double torqueIn);//力矩补偿

    /**
     * @brief moveOneStepByPosition 直接给一个目标点
     * @param positionIn
     * @return 　０不成功，前面的轨迹未完成　　１成功
     */
    int moveOneStepByPosition(double positionIn);
    int moveOneStepByPosition_plus(double positionPlus);
    int moveOneStepByPosition_prepareMix(double positionIn,double durationLeftIn);//准备被融合
    int moveOneStepByPosition_overlay(double positionIn);
    int moveOneStepByPosition_withMixer(double positionIn1, double positionIn2, double mixRatioIn);//坐标cnt用
    int moveOneStepByPosition_withMixerPre(double positionIn, double mixRatioIn);//与prepare位置融合过渡
    int moveOneStepByPosition_vel(double velIn);//axid从０开始
    /**
     * @brief moveOneStepByPV 直接给一个目标点
     * @param positionIn
     * @param velIn
     * @return
     */
    int moveOneStepByPV(double positionIn, double velIn );
    int moveOneStepByV(double velIn );
    int moveOneStepByV_filter(double velIn );
    /**
     * @brief moveOneStepByPVAT 直接给一个目标点
     * @param positionIn
     * @param velIn
     * @param accIn
     * @param torqueIn
     * @return
     */
    int moveOneStepByPVAT(double positionIn, double velIn, double accIn, double torqueIn);
    int moveOneStepByTorque(double torqueIn);


    /**
     * @brief trapMove 梯形运动，起始和终止点速度为０。保证限定　速度／加速度　不超过最大限制
     * @param positionIn　终止点
     * @param speedPercent　最大速度百分比
     * @return 　０不成功，前面的轨迹未完成　　１成功
     */
    int trapMove(double positionIn, double speedPercent, int cntIn=0);//PTP

    int trapMove(double positionIn,double speedPercent,double acceleration, int cntIn=0);//PTP
    /**
     * @brief trapMoveByTime 梯形运动，起始和终止点速度为０。保证限定　速度／加速度　不超过最大限制.
     * 运动过程时间可能比设定要长
     * @param positionIn 终止点
     * @param timeDuration 运动过程时间
     * @return
     */
    int trapMoveByTime(double positionIn,double timeDuration,double speedRatio, int cntIn=0);
    /**
     * @brief rectangularMove  恒定速度运动，速度不超过限制
     * @param positonIn
     * @param speedPercent
     * @return
     */
    int rectangularMove(double positonIn,double speedPercent, int cntIn=0);

    int splineTapeCombinedMove(double positonIn,double speedPercent, int cntIn=0);
    /**
     * @brief splineZeroMove  样条曲线运动, 起始和终止点速度/加速度为０。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionIn 终止点
     * @param speedPercent　最大速度百分比
     * @return
     */
    int splineZeroMove(double positionIn,double speedPercent, int cntIn=0);
    /**
     * @brief splineZeroMoveByTime  样条曲线运动, 起始和终止点速度/加速度为０。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionIn
     * @param timeDuration
     * @return
     */
    int splineZeroMoveByTime(double positionIn,double timeDuration, int cntIn=0);
    int sinZeroMoveByTime(double positionIn,double timeDuration, int cntIn=0);
    int improvedSplineZeroMoveByTime(double positionIn,double timeDuration, int cntIn=0);
    /**
     * @brief splineContinueMove 样条曲线运动, 终止点速度/加速度为参数值。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionIn　终止点
     * @param velIn　终止点速度
     * @param accIn　终止点加速度
     * @return
     */
    int splineContinueMove(double positionIn,double velIn, double accIn, int cntIn=0);//平滑拼接用

    //中间点过渡时，速度前馈可能出现不连续问题。
    int splineContinueMoveMultiPoint(QVector<VelPoint> velPoint);//多点通过
    /**
     * @brief splineContinueMoveByTime 样条曲线运动, 终止点速度/加速度为参数值。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionIn
     * @param velIn
     * @param accIn
     * @param timeDuration
     * @return
     */
    int splineContinueMoveByTime(double positionIn,double velIn,
                                 double accIn, double timeDuration, int cntIn=0);//平滑拼接用

    /**
     * @brief getTimeDurationTrapMove 获取轨迹运动所需要的时间，多轴同步用。
     * @param positionIn
     * @param speedPercent
     * @param timeDuration
     * @return
     */
    int getTimeDurationTrapMove(double positionIn, double speedPercent, double &timeDuration);//PTP同步用
    int getTimeDurationSinMove(double positionIn, double speedPercent, double &timeDuration);//PTP同步用
    int getTimeDurationSplineZeroMove(double positionIn,double speedPercent, double  &timeDuration);//PTP同步用
    int getTimeDurationImprovedSplineZeroMove(double positionIn,double speedPercent, double  &timeDuration);//PTP同步用
    int getTimeDurationSplineContinueMove(double positionIn,double velIn, double &timeDuration);//平滑同步用

//    /**
//     * @brief cease 暂无功能
//     * @return
//     */
//    int cease();
//    /**
//     * @brief stop　暂无功能
//     * @return
//     */
//    int stop();
    /**
     * @brief resetTraj 仅清除历史轨迹数据
     * @return
     */
//    int resetTraj();//
    /**
     * @brief resetAll 清除历史轨迹数据、错误状态、从新加载axis命令值
     * @return
     */
    int resetAll();//
    int resetSimpleTp();
    /**
     * @brief calculateFollowOffset 计算跟随误差
     * @return
     */
    int calculateFollowOffset();
    /**
     * @brief waitPositionFollowOffset  等待跟随误差直到　fabs(positionFollowOffset)小于targetFollowError
     * @return
     */
    int waitPositionFollowOffset();//
    bool isTheLastTrajCommandMoving();//是否为最后一条正在运行的轨迹
    /**
     * @brief isVelProfileEmpty  轨迹对象是否为空
     * @return ０有轨迹　１无轨迹
     */
    int isTrajComposeEmpty();
    int deleteTrajComposite();
    KDL::VelocityProfile* getTrajComposite(int trajIndex=0);

    int addNewTrajComposite(AxisTrajeAttribute attrIn);
    int setTrajCompositeEndCutoffTime(QVector<double> timeIn);
    /**
     * @brief isPositionCommandCheckOk Position限位检查
     * @param positionIn
     * @return 　-1 not OK       1 OK
     */
    int isPositionCommandCheckOk(double positionIn);

    int isPositionFeedbackCheckOk(double positionIn);

     int isPositionCommandIncrementCheckOk();

     int isVelocityFollowOffsetOk();

    /**
     * @brief isVelCommandCheckOk 限位检查
     * @param velIn
     * @return
     */
    int isVelCommandCheckOk(double velIn, bool isCheckT1=true);
    int isVelFeedbackCheckOk(double velIn, bool isCheckT1=true);
    int isVelFeedbackIncrementCheckOk(double velIn);
    int isVelCompensationCheckOk(double velIn);
    /**
     * @brief isAccCommandCheckOk 限位检查
     * @param accIn
     * @return
     */
    int isAccCommandCheckOk(double accIn);
    /**
     * @brief isTorqueCommandCheckOk 限位检查
     * @param torqueIn
     * @return
     */
    int isTorqueCommandCheckOk(double torqueIn);
    int isTorqueCompensationCheckOk(double torqueIn);
    int isTorqueFeedbackCheckOk(double torqueIn);
    /**
     * @brief isFollowOffsetCheckOk 限位检查
     * @param followOffsetIn
     * @return
     */
    int isPositionFollowOffsetCheckOk();
    /**
     * @brief isTargetOffsetCheckOk 限位检查。轨迹末尾有需要才去调用。
     * @param followOffsetIn
     * @return
     */
    int isTargetOffsetCheckOk();

    /**
     * @brief isLimitCheckOk 　位置、速度、加速度、力矩、跟随误差　同时检测，且＆＆
     * @return 　-1 not OK       1 OK
     */
    int isLimitCheckOk();//不检查反馈和偏差
    int isLimitCheckOk_logic();
    /**
     * @brief setCheckMode 　速度检查模式设定
     * @param modeIn　E_CHECK_MODE_T1　　E_CHECK_MODE_T2
     * @return
     */
    int setCheckMode(E_CHECK_MODE modeIn);

    double getAccumulatedTime(int trajIndexIn=0);
    double accumulatedTimeMoveOneStep(int trajIndexIn=0);
    int setAccumulatedTime(double timeIn,int trajIndexIn=0);
    int clearTraj();
    int clearTraj_front();
    double getPositionCommand_cnt();

    int addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);


public:
    volatile bool isServoOnStatus;
    volatile int limitCheckMode;//E_CHECK_MODE_T1　　E_CHECK_MODE_T2
//     KDL::VelocityProfile* volatile velProfile;//速度对象,//deltaTime单位影响
     QQueue<AxisTrajeAttribute>  trajAttributeQueue;
//     KDL::VelocityProfile* volatile velProfileArry[VEL_PROFILE_ARRY_MAX];//速度队列
//     QVector<double> timeDurationCache;//先进来的行，先放。多段所需要时间
//     volatile int currentVelProfileArryIndex;//不用队列时＝－１，用队列时０～VEL_PROFILE_ARRY_MAX

    volatile double timeAxisScale;//时间轴比例尺，内部求点时，需要修正v=vOLD/timeAxisScale, a=aOLD/timeAxisScale^2 .
    volatile double axisAccelerationRatio;//加速度比例
    volatile double deltaTime;//单步长
//    double accumulatedTime;//累计时长
//    double accumulatedTime_secondTraj;
    volatile int axisAbstractType;//轴抽象类型

    volatile int currentRunningLine;//当前运行行
    volatile int iscurrentAxisCommandFInished;//当前运行行是否结束
    volatile double currentCommandRemainingTime;//当前运行行剩余时间
    volatile double totalRemainingTime;//总剩余时间
     QQueue<int> linesNumberQueue;//行号队列。
     QQueue<double> linesDurationQueue;//行运行时间队列

public:
    volatile double positionCommand;//位置命令 mm degree
    volatile double positionCommand_cnt;
    double positionCommand_planed;//规划开始点
    volatile double positionCommand_prepare;//位置命令 mm degree
    volatile double positionCommandOld;//位置命令 mm degree
    volatile double velCommand;//速度命令 mm/s degree/s
    volatile double velCommand_mixed;

    volatile double accCommand;//速度命令 mm/s^2 degree/s^2
    volatile double accCommand_mixed;
    volatile double torqueCommand;//力矩命令　 N Ｎ.mm
    volatile double torqueCompensation;
    volatile double positionFeedback;//反馈位置 mm degree
    volatile double positionFeedbackOld;//反馈位置 mm degree
    volatile double velFeedback;//反馈速度 mm/s degree/s
    volatile double torqueFeedback;//反馈力  N Ｎ.mm
    volatile double positionFollowOffset;//位置跟随误差　mm degree
    volatile double velFollowOffset;//速度跟随误差　mm/s degree/s
    volatile double torqueFollowOffset;//力矩跟随误差　N Ｎ.mm

     volatile int basicStatus;//基本状态
     volatile int axisIdByRobot;//机器人关节顺序命名的轴号，axid从０开始
     volatile int isPositionCommandExceed;
     volatile int isVelCommandExceed;
     volatile int isAccCommandExceed;
     volatile int isTorqueCommandExceed;
     volatile int isProcessFollowErrorExceed;


    //-------------------------------------------------------------轴基本parameter-------------------------------
    /**
     * @brief jointName 关节名称
     */
    std::string jointName;//在公共目录下，使用std::string
    /**
     * @brief jointType 关节类型
     * ０    旋转关节
     * １    移动关节
     */
    int jointType;
    /**
     * @brief dhType
     * 0    standard dh
     * 1    modified dh
     */
    int dhType;//华磊添加　dh参数类型
    /**
     * @brief dhParameter
     * \sita    Z轴之间的夹角
     * \d       Z轴方向的偏移量
     * \a       X轴方向的偏移量
     * \alpha   X轴之间的夹角
     */
    double dhParameter[4];//华磊修改　把原来的QVector<float>dhParameter改为double dhParameter[4]
    //华磊修改　把原来的QVector<float>positionVary改为两个变量maxPosition和minPosition
    /**
     * @brief maxPosition 电机转角/位移最大值，单位角度/mm
     */
    float maxPosition;
    /**
     * @brief minPosition 电机转角/位移最小值，单位角度/mm
     */
    float minPosition;
    double approachDistance;
    /**
     * @brief positionOffset 电机位置偏移
     */
    float positionOffset;
    /**
     * @brief maxVelocity 最大转速
     */
    float maxVelocity;
    float maxFeedbackVelocity;
    /**
     * @brief maxTorque 最大转矩
     */
    float maxTorque;

    float jogTorqueLimit;
    /**
     * @brief maxAccelation 最大加速度
     */
    float maxAccelation;
    double accRatio;
    /**
     * @brief mass 关节质量
     */
    float mass;
    /**
     * @brief massCenter 关节质心
     * x y z
     */
    QVector<float>massCenter;
    /**
     * @brief inertiaMatrix 惯性张量矩阵
     * Ixx Iyy Izz Ixy Ixz Iyz
     */
    QVector<float>inertiaMatrix;
    /**
     * @brief viscousFriction 粘性摩擦常量
     */
    float viscousFriction;
    /**
     * @brief coulombFriction 库伦摩擦常量
     */
    float coulombFriction;
    /**
     * @brief mobile 能否转动
     * true 能转动
     * false 不能转动
     */
    bool mobile;
    /**
     * @brief gearRatio 电机齿轮比
     */
    float gearRatio;
    /**
     * @brief screwPitch 螺距，单位毫米
     */
    float screwPitch;
    /**
     * @brief encoderResolution 电机编码器位数
     */
    int encoderResolution;
    /**
     * @brief pulsePerRound 电机一圈的脉冲数，初始化时计算
     */
    int pulsePerRound;//华磊添加
    /**
     * @brief coupleFlag 耦合标记,其他关节是否对其产生作用
     */
    bool coupleFlag;
    /**
     * @brief coupleJoint 耦合作用的关节索引(该索引为原始影响关节)，从0开始
     */
    int coupleJoint;
    /**
     * @brief coupleReducer 对耦合作用有影响的减速比（关节　影响　关节。关节　影响　直线。　该参数２种情况都适用。正负关联补偿方向。关节　影响　直线　的情况，参数为-1 或１。）
     */
    float coupleReducer;
    /**
     * @brief 外部轴标识,是否外部轴, 0:内部轴 1:外部轴
     */
    int externAxisFlag;
    /**
     * @brief 指令精确到达的目标点允许误差
     */
    float targetFollowError;//mm ,　degree
    /**
     * @brief 跟隨過程最大允许误差
     */
    float processFollowError;//mm ,　degree

    float processVelocityFollowError;//mm/s ,　degree/s

    double t1RatioLimit;
   //-------------------------------------------------------------轴基本parameter-------------------------------

private:
     double velCompensation;//单纯速度补偿，策略控制使用
     double velCommand_byIncrement;

    MotionMessage* motionMessage;
    int robotId;
    E_MOTOR_CONTROL_MODE controlMode;
    E_MOTOR_CONTROL_MODE controlMode_old;
    FilteringAlgorithm *filteringAlgorithm;
    FilteringAlgorithm *filteringAlgorithm_qqvel;
    FilteringAlgorithm *filteringAlgorithm_position;
    E_AXIS_LOGIC_TYPE axisLogicalType;
    double filtering_interpolationRatio;
    double filtering_maxJerkBias;
    int maxErrorCount;
    int minErrorCount;
    double vel_old;
    AxisMixControlInfo mixControlInfo;
    double idealTorque;
    E_AXIS_PLAN_MODE axisPlanMode;
    simple_tp_t *simpleTp;
};

#endif // AxisOBJECT_H
